/**
 *************************(C) COPYRIGHT 2024 DragonBot*************************
 * @file       bsp_can.c/h
 * @brief
 * @note
 * @history
 * Date            Author          Modification
 * 2023-10-26      wanghongxi      代码框架
 * 2024-08-30      ZouAjie	       优化
 @verbatim
 ==============================================================================

 ==============================================================================
 @endverbatim
 *************************(C) COPYRIGHT 2024 DragonBot*************************
 */
#include "bsp_can.h"
#include "user_lib.h"

/*---------------------------------------------------------------------------------------------------*/
static void get_motor_measure(motor_measure_t *ptr, uint8_t *data);
static void get_shaft_angle(motor_measure_t *ptr, motor_type_e motor);
/*---------------------------------------------------------------------------------------------------*/
motor_measure_t chassis_motor[4] = {0}; // 底盘3508电机数据[0:3]
motor_measure_t trigger_motor = {0};	// 2006拨弹电机
motor_measure_t fric_motor[2] = {0};	// 3508摩擦轮电机[0:1]
motor_measure_t gimbal_motor[2] = {0};	// 6020云台电机[0:1]

static CAN_TxHeaderTypeDef gimbal_tx_message;  // 云台CAN发送结构体
static uint8_t gimbal_can_send_data[8];		   // 云台CAN发送数据
static CAN_TxHeaderTypeDef chassis_tx_message; // 底盘CAN发送结构体
static uint8_t chassis_can_send_data[8];	   // 底盘CAN发送数据
static CAN_TxHeaderTypeDef shoot_tx_message;   // 拨弹CAN发送结构体
static uint8_t shoot_can_send_data[8];		   // 拨弹CAN发送数据
/*---------------------------------------------------------------------------------------------------*/
/**
 * @brief  CAN过滤器初始化
 * @param  None
 */
void CAN_Config(void)
{
	CAN_FilterTypeDef can_filter_st;								   // CAN滤波器结构体
	can_filter_st.FilterActivation = ENABLE;						   // 使能滤波器
	can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;				   // ID屏蔽模式
	can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;				   // 32位滤波器
	can_filter_st.FilterIdHigh = 0x0000;							   // 32位ID
	can_filter_st.FilterIdLow = 0x0000;								   // 32位ID
	can_filter_st.FilterMaskIdHigh = 0x0000;						   // 32位屏蔽ID
	can_filter_st.FilterMaskIdLow = 0x0000;							   // 32位屏蔽ID
	can_filter_st.FilterBank = 0;									   // 过滤器组0
	can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;				   // 过滤器组0关联到FIFO0
	HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);					   // 配置过滤器
	HAL_CAN_Start(&hcan1);											   // 开启CAN1
	HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); // 开启中断

	can_filter_st.SlaveStartFilterBank = 14;						   // 从过滤器组14开始
	can_filter_st.FilterBank = 14;									   // 过滤器组14
	HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);					   // 配置过滤器
	HAL_CAN_Start(&hcan2);											   // 开启CAN2
	HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING); // 开启中断
}

/**
 * @brief      hal库CAN回调函数,接收电机数据,并将数据解包后存入相应数组
 * @param[in]  hcan:CAN句柄指针
 * @retval     none
 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	if (hcan == &hcan1)
	{
		CAN_RxHeaderTypeDef RxHead;
		uint8_t Rxdata[8];
		HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHead, Rxdata);
		switch (RxHead.StdId) // 根据StdID筛选出底盘M3508电机
		{
		case CAN_CHASSIS_M1_ID:
		case CAN_CHASSIS_M2_ID:
		case CAN_CHASSIS_M3_ID:
		case CAN_CHASSIS_M4_ID:
		{
			static uint8_t i = 0;
			i = RxHead.StdId - CAN_CHASSIS_M1_ID;		  // 获取电机ID
			get_motor_measure(&chassis_motor[i], Rxdata); // 获取电机原始数据
			get_shaft_angle(&chassis_motor[i], M3508);	  // 获取输出轴角度
			break;
		}
		default:
			break;
		}
	}
	else if (hcan == &hcan2)
	{
		
				CAN_RxHeaderTypeDef RxHead;
		uint8_t Rxdata[8];
		HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHead, Rxdata);
		switch (RxHead.StdId)
		{
		// 摩擦轮M3508电机
		case CAN_SHOOT_FRIC_M1_ID:
			get_motor_measure(&fric_motor[0], Rxdata); // 获取电机原始数据
			break;
		case CAN_SHOOT_FRIC_M2_ID:
			get_motor_measure(&fric_motor[1], Rxdata); // 获取电机原始数据
			break;
		// 拨弹轮M2006电机
		case CAN_SHOOT_TRIGGER_ID:
			get_motor_measure(&trigger_motor, Rxdata); // 获取电机原始数据
			get_shaft_angle(&trigger_motor, M2006);	   // 获取输出轴角度
			break;
		// 云台YAW轴M6020电机
		case CAN_GIMBAL_YAW_ID:
			get_motor_measure(&gimbal_motor[0], Rxdata);
			get_shaft_angle(&gimbal_motor[0], M6020); // 获取输出轴角度
			break;
		// 云台PITCH轴M6020电机
		case CAN_GIMBAL_PITCH_ID:
			get_motor_measure(&gimbal_motor[1], Rxdata);
			get_shaft_angle(&gimbal_motor[1], M6020); // 获取输出轴角度
			break;
		default:
			break;
		}
	}
}

/**
 * @brief      解包电机数据
 * @param[in]  *ptr:电机数据地址
 * @param[in]  *data:原始数据
 * @retval     none
 */
static void get_motor_measure(motor_measure_t *ptr, uint8_t *data)
{
	ptr->last_ecd = ptr->ecd;
	ptr->ecd = (uint16_t)(data[0] << 8 | data[1]);
	ptr->speed_rpm = (uint16_t)(data[2] << 8 | data[3]);
	ptr->given_current = (uint16_t)(data[4] << 8 | data[5]);
	ptr->temperate = data[6];
}

/**
 * @brief      电机输出轴角度换算
 * @param[in]  *ptr: 电机数据地址
 * @param[in]  motor: 电机类型
 * @retval     none
 */
static void get_shaft_angle(motor_measure_t *ptr, motor_type_e motor)
{
	// 上电获取编码器补偿量
	if (ptr->init_cnt < 10)
	{
		ptr->offset_ecd = ptr->ecd;
		ptr->init_cnt++;
	}
	else
	{
		switch (motor) // 根据不同电机类型计算输出轴角度
		{
		case M2006:
			// 记电机轴圈数
			if (ptr->ecd - ptr->last_ecd > 4096)
				ptr->round_cnt--;
			else if (ptr->ecd - ptr->last_ecd < -4096)
				ptr->round_cnt++;
			if (ptr->round_cnt == 36 || ptr->round_cnt == -36)
				ptr->round_cnt = 0;
			// 算当前电机轴总编码器值
			ptr->total_angle = ptr->round_cnt * 8192 + ptr->ecd - ptr->offset_ecd;
			// 输出轴角度 = -PI到PI的循环限幅(电机轴总编码器值 * 转换比)
			ptr->shaft_angle = rad_format(ptr->total_angle * M2006_CONVERSION_RATIO);
			break;
		case M3508:
			// 记电机轴圈数
			if (ptr->ecd - ptr->last_ecd > 4096)
				ptr->round_cnt--;
			else if (ptr->ecd - ptr->last_ecd < -4096)
				ptr->round_cnt++;
			if (ptr->round_cnt == 3591 || ptr->round_cnt == -3591)
				ptr->round_cnt = 0;
			// 算当前电机轴总编码器值
			ptr->total_angle = ptr->round_cnt * 8192 + ptr->ecd - ptr->offset_ecd;
			// 输出轴角度 = -PI到PI的循环限幅(电机轴总编码器值 * 转换比)
			ptr->shaft_angle = rad_format(ptr->total_angle * M3508_CONVERSION_RATIO);
			break;
		case M6020:
			ptr->shaft_angle = rad_format(ptr->ecd * M6020_CONVERSION_RATIO);
			break;
		}
	}
}

/**
 * @brief      发送底盘电机控制电流(0x201,0x202,0x203,0x204)
 * @param[in]  motor1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
 * @param[in]  motor2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
 * @param[in]  motor3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
 * @param[in]  motor4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
 * @retval     none
 */
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
	uint32_t send_mail_box;						   // 发送邮箱
	chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID; // 发送ID
	chassis_tx_message.IDE = CAN_ID_STD;
	chassis_tx_message.RTR = CAN_RTR_DATA;
	chassis_tx_message.DLC = 0x08;
	chassis_can_send_data[0] = motor1 >> 8;
	chassis_can_send_data[1] = motor1;
	chassis_can_send_data[2] = motor2 >> 8;
	chassis_can_send_data[3] = motor2;
	chassis_can_send_data[4] = motor3 >> 8;
	chassis_can_send_data[5] = motor3;
	chassis_can_send_data[6] = motor4 >> 8;
	chassis_can_send_data[7] = motor4;
	HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
 * @brief      发送电机控制电流(0x205,0x206,0x207,0x208)
 * @param[in]  yaw: (0x205) 6020电机控制电流, 范围 [-30000,30000]
 * @param[in]  pitch: (0x206) 6020电机控制电流, 范围 [-30000,30000]
 * @retval     none
 */
void CAN_cmd_gimbal(int16_t yaw, int16_t pitch)
{
	uint32_t send_mail_box;						 // 发送邮箱
	gimbal_tx_message.StdId = CAN_GIMBAL_ALL_ID; // 发送ID
	gimbal_tx_message.IDE = CAN_ID_STD;
	gimbal_tx_message.RTR = CAN_RTR_DATA;
	gimbal_tx_message.DLC = 0x08;
	gimbal_can_send_data[0] = (yaw >> 8);
	gimbal_can_send_data[1] = yaw;
	gimbal_can_send_data[2] = (pitch >> 8);
	gimbal_can_send_data[3] = pitch;
	gimbal_can_send_data[4] = 0;
	gimbal_can_send_data[5] = 0;
	gimbal_can_send_data[6] = 0;
	gimbal_can_send_data[7] = 0;
	HAL_CAN_AddTxMessage(&GIMBAL_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

/**
 * @brief      发送发射装置电机控制电流(0x201,0x202,0x203,0x204)
 * @param[in]  fric1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
 * @param[in]  fric2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
 * @param[in]  trigger: (0x203) 2006电机控制电流, 范围 [-10000,10000]
 */
void CAN_cmd_shoot(int16_t fric1, int16_t fric2, int16_t trigger)
{
	uint32_t send_mail_box;					   // 发送邮箱
	shoot_tx_message.StdId = CAN_SHOOT_ALL_ID; // 发送ID
	shoot_tx_message.IDE = CAN_ID_STD;
	shoot_tx_message.RTR = CAN_RTR_DATA;
	shoot_tx_message.DLC = 0x08;
	shoot_can_send_data[0] = fric1 >> 8;
	shoot_can_send_data[1] = fric1;
	shoot_can_send_data[2] = fric2 >> 8;
	shoot_can_send_data[3] = fric2;
	shoot_can_send_data[4] = trigger >> 8;
	shoot_can_send_data[5] = trigger;
	shoot_can_send_data[6] = 0;
	shoot_can_send_data[7] = 0;
	HAL_CAN_AddTxMessage(&SHOOT_CAN, &shoot_tx_message, shoot_can_send_data, &send_mail_box);
}

/**
 * @brief      返回底盘 3508电机数据指针
 * @param[in]  i: 电机编号,范围[0,3]
 * @retval     底盘电机数据指针
 */
const motor_measure_t *get_chassis_motor_measure_point(uint8_t i)
{
	return &chassis_motor[(i & 0x03)];
}

/**
 * @brief      返回云台yaw 6020电机数据指针
 * @param[in]  none
 * @retval     电机数据指针
 */
const motor_measure_t *get_gimbal_yaw_motor_measure_point(void)
{
	return &gimbal_motor[0];
}

/**
 * @brief      返回云台pitch 6020电机数据指针
 * @param[in]  none
 * @retval     电机数据指针
 */
const motor_measure_t *get_gimbal_pitch_motor_measure_point(void)
{
	return &gimbal_motor[1];
}

/**
 * @brief      返回拨弹 2006电机数据指针
 * @param[in]  none
 * @retval     电机数据指针
 */
const motor_measure_t *get_trigger_motor_measure_point(void)
{
	return &trigger_motor;
}

/**
 * @brief      返回摩擦轮 3508电机数据指针
 * @param[in]  i: 电机编号,范围[0,1]
 * @retval     摩擦轮电机数据指针
 */
const motor_measure_t *get_fric_motor_measure_point(uint8_t i)
{
	return &fric_motor[(i & 0x01)];
}

/* 拨弹盘角度校准 */
void TriggerMotorCalibrate(void)
{
	trigger_motor.init_cnt = 0;
}
